#include "H/Function_Init.H"


bit sev_claw_en,  claw_tm_flag;

uint16_t pwm_value, ctrl_cnt;

void Motor_1_ctrl(uint8_t mtn)
{
	switch(mtn)
	{
		case M_FWD:	P00 = 1; P01 = 0;	break;
		case M_RESV:P00 = 0; P01 = 1;	break;
		default:	P00 = 0; P01 = 0;	break;
	}
}

void Motor_2_ctrl(uint8_t mtn)
{
	switch(mtn)
	{
		case M_FWD:	P02 = 1; P03 = 0;	break;
		case M_RESV:P02 = 0; P03 = 1;	break;
		default:	P02 = 0; P03 = 0;	break;
	}
}

void Motor_3_ctrl(uint8_t mtn)
{
	switch(mtn)
	{
		case M_FWD:	P04 = 1; P05 = 0;	break;
		case M_RESV:P04 = 0; P05 = 1;	break;
		default:	P04 = 0; P05 = 0;	break;
	}
}


void Motor_Run(void)
{
	if(3 >= roll_x)
		Motor_3_ctrl(M_FWD);
	else if(7 <= roll_x)
		Motor_3_ctrl(M_RESV);
	else
		Motor_3_ctrl(M_STOP);
	
	if(3 >= roll_y)
		Motor_2_ctrl(M_FWD);
	else if(7 <= roll_y)
		Motor_2_ctrl(M_RESV);
	else
		Motor_2_ctrl(M_STOP);
	
//	if(Button_M)
}

void Claw_Ctrl_TmInter(void)
{
	claw_tm_flag = 1;
}
uint8_t sevro_value, sevro_value_pwm;
void SevroSet(uint8_t arg)
{
	if(20 > arg)arg = 20;
	else if(40 < arg)arg = 40;
	sevro_value = arg;
}
uint16_t sev_cnt;
void Sevro_Deal(void)		// 1~2ms, cycle 20ms
{
	
	P40 = ((sev_cnt < sevro_value)? 1:0);
	
	sev_cnt++;
	if(400 <= sev_cnt)sev_cnt = 0;
}

uint8_t ctrl_ccnt;
uint8_t mtn=1;
void Claw_Ctrl(void)
{
	sev_claw_en = 1;
	if(!sev_claw_en)return;
	if(!claw_tm_flag)return;
	claw_tm_flag = 0;			// 1ms
	
	ctrl_ccnt++;
	if(40 > ctrl_ccnt)return;
	ctrl_ccnt = 0;
	
	
	switch(mtn)
	{
		case 1:{
			
			if(40 > sevro_value_pwm)sevro_value_pwm++;
			else
				mtn = 2;
		}break;
		
		case 2:{
			if(20 < sevro_value_pwm)sevro_value_pwm--;
			else
				mtn = 1;
		}break;
	}
	SevroSet(sevro_value_pwm);
}

